//-----------------------------------------------------------------------
//  This file is part of the Microsoft Robotics Studio Code Samples.
// 
//  Copyright (C) Microsoft Corporation.  All rights reserved.
//
//  $File: visionprocessing.cs $ $Revision: 1 $
//-----------------------------------------------------------------------

using System.IO;
using Microsoft.Robotics.PhysicalModel;
using System;
using System.Drawing;

namespace ProMRDS.SimpleSoccerPlayer
{

    public class ImageProcessingResult
    {
        public double Factor;
        public int Area;
        public float AverageHue;
        public float AverageSaturation;
        public int xSecondMoment;
        public int ySecondMoment;
        public Vector2 Center;
    }

    public static class ImageProcessing
    {
        public static int CalculateSecondMoment(int[] projection, int mean, int count)
        {
            int length = projection.Length;
            int moment = 0;

            for (int index = 0; index < length; index++)
            {
                if (projection[index] > 0)
                {
                    int normalized = index - mean;

                    moment += normalized * normalized * projection[index];
                }
            }

            return moment / count;
        }

        /// <summary>
        /// Find an object in the image, using the thresholds provided
        /// </summary>
        /// <param name="width"></param>
        /// <param name="height"></param>
        /// <param name="areaThreshold"></param>
        /// <param name="saturationThreshold"></param>
        /// <param name="hueThreshold"></param>
        /// <param name="rgbValues"></param>
        /// <param name="result"></param>
        /// <returns></returns>
        public static bool ProcessFrameHandler(
            float width,
            float height,
            float areaThreshold,
            float saturationThreshold,
            float hueThreshold,
            byte[] rgbValues,
            ImageProcessingResult result)
        {
            bool objectFound = false;
            float avgHue = 0;
            float avgSaturation = 0;
            int area = 0;
            int offset = 0;

            int[] yProjection = new int[(int)height];
            int[] xProjection = new int[(int)width];

            int xMean = 0;
            int yMean = 0;

            for (int y = 0; y < height; y++)
            {
                for (int x = 0; x < width; x++)
                {
                    //int luma = 0;
                    int r, g, b;

                    b = rgbValues[offset++];
                    g = rgbValues[offset++];
                    r = rgbValues[offset++];

                    Color color = Color.FromArgb(r, g, b);

                    float hue = color.GetHue();
                    float saturation = color.GetSaturation();
                    avgHue += hue;
                    avgSaturation += saturation;

                    if (hue < hueThreshold &&
                        saturation > saturationThreshold)
                    {
                        area++;
                        xProjection[x]++;
                        yProjection[y]++;
                        xMean += x;
                        yMean += y;
                    }
                }
            }

            double factor = 0;
            int ySecondMoment = 0;
            int xSecondMoment = 0;

            if (area > areaThreshold)
            {

                xMean = xMean / area;
                yMean = yMean / area;
                avgSaturation = avgSaturation / (height * width);
                avgHue = avgHue / (height * width);

                ySecondMoment = CalculateSecondMoment(yProjection, yMean, area);
                xSecondMoment = CalculateSecondMoment(xProjection, xMean, area);

                if (ySecondMoment > 0 && xSecondMoment > 0)
                {
                    factor = area / Math.Sqrt(ySecondMoment * xSecondMoment);
                    objectFound = true;
                }
            }

            result.Area = area;
            result.Factor = factor;
            result.xSecondMoment = xSecondMoment;
            result.ySecondMoment = ySecondMoment;
            result.Center.X = xMean;
            result.Center.Y = yMean;
            result.AverageHue = avgHue;
            result.AverageSaturation = avgSaturation;

            return objectFound;
        }


    }
}